﻿using ControlCenter.Communication;
using ControlCenter.Sensors;
using log4net;
using NetduinoRobot.Common;
using System;
using System.Collections.Generic;
using System.Configuration;
using System.Text;
using System.Windows.Forms;

namespace ControlCenter
{
    /// <summary>
    /// the main drive controller
    /// Written by: Salvador Richter
    /// Appache License Version 2.0 
    /// </summary>
    class DriveController
    {
        private const char CLOSING_BRACE = ')';
        private const string UPDATE_COMMAND = "NetduinoHappyHour.Util.UpdateRunningCommand(";
        private const string UPDATE_PAUSED_TRUE = "IsPaused;true;bool";
        private const string UPDATE_PAUSED_FALSE = "IsPaused;false;bool";
        private const string UPDATE_VALID_TRUE = "IsValid;true;bool";
        private const string UPDATE_VALID_FALSE = "IsValid;false;bool";
        private const string EXECUTABLE_COMMANDS = "NetduinoHappyHour.Util.ExecutableCommands()";
        private const string READ_CONFIG = "NetduinoHappyHour.Util.ReadSystemConfig";
        private const string READ_STATUS = "NetduinoHappyHour.Util.ReadSystemStatus";
        private const string RESET_STATUS = "NetduinoHappyHour.Util.ResetSystemStatus";
        private const string INIT_COMMAND = "NetduinoHappyHour.Hardware.DriveCommandDualAsync(1000)";
        private const int MINIMUM_TILT_ANGLE = 3;
        private const int DEFAULT_STEP_DURATION = 35;
        private const int MINIMUM_STEP_DURATION = 2;
        private DriveState _driveState = new DriveState();
        private CommandReadWrite _commandReadWrite = null;
        public DeviceReader Reader { get; private set; }
        /// <summary>
        /// Used to store some information to be set on display
        /// </summary>
        /// // todo remove
        public string Message { get; private set; }
        private static DriveController _remoteController = null;

        private DriveController()
        {
            string port = ConfigurationManager.AppSettings["btComPortName"];
            int speed = int.Parse(ConfigurationManager.AppSettings["btComPortSpeed"]);
            try
            {
                _commandReadWrite = new CommandReadWrite(new SerialPortWrapper(port, speed));
                Message = "Connected to robot";
            }
            catch (System.IO.IOException ioEx)
            {
                ILog log = LogManager.GetLogger("Error");
                string msg = "Cannot connect to robot, probably cannot open Com port " + port + "/" + speed;
                log.Warn(msg);
                Message = msg;
            }
            Reader = null;
            try
            {
                Reader = new DeviceReader(NetduinoRobot.Common.Filter.FilterType.Linear);
            }
            catch (Exception ex)
            {
                ILog log = LogManager.GetLogger("Error");
                log.Info("No Sensors found");
            }
        }

        //todo create dispose method

        public static DriveController Instance()
        {
            if (_remoteController == null)
            {
                _remoteController = new DriveController();
            }
            return _remoteController;
        }

        public bool TabletEnabled()
        {
            return Reader != null;
        }

        /// <summary>
        /// Sends command to robot, Message is to be compiled to be understood by Interactive
        /// e.g. "CommandToBeExecuted;Param1;Param2;ParamN2"
        /// </summary>
        /// <param name="commandToBeSent">Message to be send to the robot</param>
        public void SendRemoteCommand(string commandToBeSent)
        {
            ILog log = LogManager.GetLogger("Communication");
            log.Debug(commandToBeSent);

            if (_commandReadWrite != null)
            {
                //Todo check how to handle possible return values
                _commandReadWrite.WriteXmlCommand(commandToBeSent);
            }
        }

        /// <summary>
        /// Reads command reply from robot
        /// </summary>
        /// <returns>Message read from robot</returns>
        public string ReadRemoteCommand()
        {
            string s = null;
            if (_commandReadWrite != null)
            {
                //Todo check how to handle possible return values
                s = _commandReadWrite.ReadXmlCommand();
            }
            else
            {
                s = "Test aaa" + CommandHelper.CommandSeparator + "Test BBB";
            }
            return s;
        }

        /// <summary>
        /// Retrieves all executable commands from robot
        /// </summary>
        /// <returns>Names of all executable commands from the robot</returns>
        public string[] ExecutableCommands()
        {
            Message = "get executable commands";
            string s = null;
            string[] commands = null;
            SendRemoteCommand(EXECUTABLE_COMMANDS);
            s = ReadRemoteCommand();
            commands = s.Split(new char[] { CommandHelper.CommandSeparator });
            return commands;
            //return new String[]{"Test aaa","Test BBB"};
        }

        /// <summary>
        /// Retrieves system configuration from robot
        /// </summary>
        /// <returns>All variable configurations from the robot</returns>
        public string[] ReadSystemConfig()
        {
            Message = "read system config";
            string s = null;
            string[] commands = null;
            SendRemoteCommand(READ_CONFIG);
            s = ReadRemoteCommand();
            commands = s.Split(new char[] { CommandHelper.CommandSeparator });
            return commands;
            //return new String[]{"Test aaa","Test BBB"};
        }

        /// <summary>
        /// Retrieves system status from robot
        /// </summary>
        /// <returns>Actual system status from the robot</returns>
        public string[] ReadSystemStatus()
        {
            Message = "read system status";
            string s = null;
            string[] commands = null;
            SendRemoteCommand(READ_STATUS);
            s = ReadRemoteCommand();
            commands = s.Split(new char[] { CommandHelper.CommandSeparator });
            return commands;
            //return new String[]{"Test aaa","Test BBB"};
        }

        /// <summary>
        /// Retrieves system status from robot
        /// </summary>
        /// <returns>Actual system status from the robot</returns>
        public void ResetSystemStatus()
        {
            Message = "reset system status";
            SendRemoteCommand(RESET_STATUS);
        }

        /// <summary>
        /// drives the robot by cursor control
        /// </summary>
        /// <param name="direction">Left, Right, Up, Down</param>
        public void DriveByCursor(Keys direction)
        {
            StringBuilder sBuilder = new StringBuilder("DriveCommand(");
            int steps = 100;
            int duration = 2;
            switch (direction)
            {
                case Keys.Left:
                    sBuilder.Append(-steps);
                    sBuilder.Append(',');
                    sBuilder.Append(steps);
                    sBuilder.Append(',');
                    sBuilder.Append(duration);
                    sBuilder.Append(')');
                    break;
                case Keys.Right:
                    sBuilder.Append(steps);
                    sBuilder.Append(',');
                    sBuilder.Append(-steps);
                    sBuilder.Append(',');
                    sBuilder.Append(duration);
                    sBuilder.Append(')');
                    break;
                case Keys.Up:
                    sBuilder.Append(steps);
                    sBuilder.Append(',');
                    sBuilder.Append(steps);
                    sBuilder.Append(',');
                    sBuilder.Append(duration);
                    sBuilder.Append(')');
                    break;
                case Keys.Down:
                    sBuilder.Append(-steps);
                    sBuilder.Append(',');
                    sBuilder.Append(-steps);
                    sBuilder.Append(',');
                    sBuilder.Append(duration);
                    sBuilder.Append(')');
                    break;
                default:
                    break;
            }
            SendRemoteCommand(sBuilder.ToString());
            Message = direction.ToString();
        }

        /// <summary>
        /// Drives the robot interactive using tablets tilting sensors.
        /// Sends Commands to the robot.
        /// </summary>
        public void DriveByTiltingSensor()
        {
            float x;
            float y;
            float z;
            // initial value
            int speedLeft = DEFAULT_STEP_DURATION;
            int speedRight = DEFAULT_STEP_DURATION;
            bool directionForwardL = true;
            bool directionForwardR = true;
            List<string> updateCommands = new List<string>();

            if (!_driveState.Initialized)
            {
                // create initial endless running DriveComand, long step duration to prevent starting of
                SendRemoteCommand(INIT_COMMAND);
                Message = "init";
                SendRemoteCommand(UPDATE_COMMAND + UPDATE_PAUSED_TRUE + CLOSING_BRACE);
                _driveState.Initialized = true;
            }
                                                                                                            
            Reader.DoReading();
            //_commandReadWrite._file.WriteLine(Math.Abs(Reader.XValue));
            x = Reader.XValue;
            y = Reader.YValue;
            z = Reader.ZValue;

            // Check Forward or backward regardless side angle
            if (Math.Abs(y) > MINIMUM_TILT_ANGLE)
            {
                speedLeft -= (int)Math.Max(Math.Abs(y), Math.Abs(x));
                speedRight -= (int)Math.Max(Math.Abs(y), Math.Abs(x));

                directionForwardL = 0 > y;
                directionForwardR = 0 > y;
            }
            // if forward or backward and side angle correct speed to drive curve
            if (Math.Abs(y) > MINIMUM_TILT_ANGLE && Math.Abs(x) > MINIMUM_TILT_ANGLE)
            {
                speedLeft -= (int)x;
                speedRight += (int)x;
            }
            // if only side angle create point turning
            if (Math.Abs(y) < MINIMUM_TILT_ANGLE && Math.Abs(x) > MINIMUM_TILT_ANGLE)
            {
                speedLeft -= (int)Math.Abs(x);
                speedRight -= (int)Math.Abs(x);

                directionForwardL = 0 < x;
                directionForwardR = 0 > x;
            }
            // if no angle set speed zero
            if (Math.Abs(y) < MINIMUM_TILT_ANGLE && Math.Abs(x) < MINIMUM_TILT_ANGLE)
            {
                speedLeft = 0;
                speedRight = 0;
            }

            if (speedLeft == 0 || speedRight == 0)
            {
                // set both sides zero if one is zero
                AddNotNullElement(updateCommands, _driveState.GetPause(speedLeft, speedRight));
                _driveState.GetSpeedLeft(0);
                _driveState.GetSpeedRight(0);
            }
            else
            {
                // avoid zero or negative duration
                if (speedLeft < MINIMUM_STEP_DURATION)
                {
                    speedLeft = MINIMUM_STEP_DURATION;
                }
                if (speedRight < MINIMUM_STEP_DURATION)
                {
                    speedRight = MINIMUM_STEP_DURATION;
                }
                AddNotNullElement(updateCommands, _driveState.GetPause(speedLeft, speedRight));
                
                AddNotNullElement(updateCommands, _driveState.GetSpeedLeft(speedLeft));
                AddNotNullElement(updateCommands, _driveState.GetSpeedRight(speedRight));

                AddNotNullElement(updateCommands, _driveState.GetForwardLeft(directionForwardL));
                AddNotNullElement(updateCommands, _driveState.GetForwardRight(directionForwardR));
            }

            if (updateCommands.Count > 0)
            {
                StringBuilder sBuilder = new StringBuilder();
                sBuilder.Append(UPDATE_COMMAND);
                foreach (string item in updateCommands)
                {
                    sBuilder.Append(item);
                    sBuilder.Append('#');
                }
                //remove needless added hash
                sBuilder.Remove(sBuilder.Length - 1, 1);
                //add closing brace
                sBuilder.Append(CLOSING_BRACE);
                //string s = sBuilder.ToString();
                SendRemoteCommand(sBuilder.ToString());
            }
        }

        public void StopDrive()
        {
            SendRemoteCommand(UPDATE_COMMAND + UPDATE_VALID_FALSE + CLOSING_BRACE);
            _driveState.Initialized = false;
        }

        /// <summary>
        /// Closes the remote connection to the robot. Next call on Instance() will create new DriveController
        /// </summary>
        public void Close()
        {
            StopDrive();
            //_commandReadWrite.CloseComPort();
            _remoteController = null;
        }

        /// <summary>
        /// adds myString to myList only if myString is not null or empty
        /// </summary>
        /// <param name="myList">list to be filled (in out prameter)</param>
        /// <param name="myString">value to be added</param>
        private void AddNotNullElement(List<string> myList, string myString)
        {
            if (myString != null && !myString.Equals(""))
            {
                myList.Add(myString);
            }
        }

        private class DriveState
        {
            private int _speedLeft = 0;
            private int _speedRight = 0;
            private bool _forwardLeft = true;
            private bool _forwardRight = true;
            public bool Initialized { get; set; }

            public DriveState()
            {
                Initialized = false;
            }

            //todo check it
            public string GetPause(int speedL, int speedR)
            {
                // reactivation required
                if ((_speedLeft == 0 || _speedRight == 0) && speedL != 0 && speedR != 0)
                {
                    return UPDATE_PAUSED_FALSE;
                }
                if ((speedL == 0 || speedR == 0) && _speedLeft != 0 && _speedRight != 0)
                {
                    return UPDATE_PAUSED_TRUE;
                }
                return null;
            }

            public string GetSpeedLeft(int speed)
            {
                string returnValue = null;

                if (speed != _speedLeft)
                {
                    if (speed == 0)
                    {
                        returnValue = UPDATE_PAUSED_TRUE;
                    }
                    else
                    {
                        returnValue = "StepDurationLeft;" + speed + ";int";
                    }
                    _speedLeft = speed;
                }
                return returnValue;
            }
            public string GetSpeedRight(int speed)
            {
                string returnValue = null;

                if (speed != _speedRight)
                {
                    if (speed == 0)
                    {
                        returnValue = UPDATE_PAUSED_TRUE;
                    }
                    else
                    {
                        returnValue += "StepDurationRight;" + speed + ";int";
                    }
                    _speedRight = speed;
                }
                return returnValue;
            }
            public string GetForwardLeft(bool forward)
            {
                if (forward != _forwardLeft)
                {
                    _forwardLeft = forward;
                    return "IsDirectionForwardLeft;" + forward + ";bool";
                }
                return null;
            }
            public string GetForwardRight(bool forward)
            {
                if (forward != _forwardRight)
                {
                    _forwardRight = forward;
                    return "IsDirectionForwardRight;" + forward + ";bool";
                }
                return null;
            }
        }
    }
}
